/*
 * PointCloud.cpp
 *
 *  Created on: Feb 15, 2011
 *      Author: johang
 */

#ifndef POINTCLOUD_H_
#include "PointCloud.h"
/*#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/ap.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/alglibinternal.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/alglibmisc.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/linalg.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/solvers.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/optimization.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/specialfunctions.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/integration.cpp"
#include "../thirdparty/alglib-3.3.0.cpp/cpp/src/interpolation.cpp"*/

//#include "contextLearner.h"

  IplImage * work_image_rgb = 0;
  void
  show_rgb_matrix(float ** r, float ** g, float ** b, int sx, int sy)
  {
    float min = 10000000;
    float max = -1000000;
    for (int i = 0; i < sx; i++)
      {
        for (int j = 0; j < sy; j++)
          {
            if (r[i][j] != -123)
              {
                if (r[i][j] < min)
                  {
                    min = r[i][j];
                  }
                else if (r[i][j] > max)
                  {
                    max = r[i][j];
                  }
              }
            if (g[i][j] != -123)
              {
                if (g[i][j] < min)
                  {
                    min = g[i][j];
                  }
                else if (g[i][j] > max)
                  {
                    max = g[i][j];
                  }
              }

            if (b[i][j] != -123)
              {
                if (b[i][j] < min)
                  {
                    min = b[i][j];
                  }
                else if (b[i][j] > max)
                  {
                    max = b[i][j];
                  }
              }
          }
      }
    float mul = 255.0f / (max - min);
    if (work_image_rgb != 0)
      cvReleaseImage(&work_image_rgb);
    work_image_rgb = cvCreateImage(cvSize(sx, sy), IPL_DEPTH_8U, 3);
    for (int i = 0; i < sx; i++)
      {
        for (int j = 0; j < sy; j++)
          {
            if (b[i][j] != -123)
              {
                int valb = (b[i][j] - min) * mul;
                work_image_rgb->imageData[3 * (j * sx + i) + 0] = valb;

              }
            else
              {
                work_image_rgb->imageData[3 * (j * sx + i) + 0] = 0;
              }

            if (g[i][j] != -123)
              {
                int valg = (g[i][j] - min) * mul;
                work_image_rgb->imageData[3 * (j * sx + i) + 1] = valg;

              }
            else
              {
                work_image_rgb->imageData[3 * (j * sx + i) + 1] = 0;
              }

            if (r[i][j] != -123)
              {
                int valr = (r[i][j] - min) * mul;
                work_image_rgb->imageData[3 * (j * sx + i) + 2] = valr;

              }
            else
              {
                work_image_rgb->imageData[3 * (j * sx + i) + 2] = 0;
              }
          }
      }
    //cvNamedWindow("rgb image", CV_WINDOW_AUTOSIZE);
    //cvShowImage("rgb image", work_image_rgb);
    //cvWaitKey(100);
  }

/* --------------------------------------------------------------------------*/
PointCloud::PointCloud(int width, int height) :
  m_width(width), m_height(height)
{

  Point defaultPoint;
  defaultPoint.is_object = false;
  defaultPoint.norm_x = 1;
  defaultPoint.norm_y = 1;
  defaultPoint.norm_z = 1;

  m_points = new Point*[width];

  for (int i = 0; i < m_width; i++)
    {
    m_points[i] = new Point[m_height];
      for (int j = 0; j < m_height; j++)
        {
        m_points[i][j] = defaultPoint;
        }
    }
  bad_point = std::numeric_limits<float>::quiet_NaN();
}

/* --------------------------------------------------------------------------*/
/*
using namespace alglib;

void function_cx_1_func(const real_1d_array &c, const real_1d_array &x, double &func, void *ptr) 
{
	float tx = x[0];
	float ty = x[1];
	func = c[0]+c[1]*tx+c[2]*ty;
}

void function_cx_1_grad(const real_1d_array &c, const real_1d_array &x, double &func, real_1d_array &grad, void *ptr) 
{
	float tx = x[0];
	float ty = x[1];
	func = c[0]+c[1]*tx+c[2]*ty;
	grad[0] = tx;
	grad[1] = ty;
}
void function_cx_1_hess(const real_1d_array &c, const real_1d_array &x, double &func, real_1d_array &grad, real_2d_array &hess, void *ptr) 
{
    func = exp(-c[0]*pow(x[0],2));
    grad[0] = -pow(x[0],2)*func;
    hess[0][0] = pow(x[0],4)*func;
}*/


void
PointCloud::filterXYZ()
{
/*	struct timeval start, end;
    	long mtime, seconds, useconds;
    	mtime = 0;
	gettimeofday(&start, NULL);
	
	float ** tot_x = new float*[m_width];
	float ** tot_y = new float*[m_width];
	float ** tot_z = new float*[m_width];
	float ** sum_x = new float*[m_width];
	float ** sum_y = new float*[m_width];
	float ** sum_z = new float*[m_width];
	for(int i = 0; i < m_width;i++){
		tot_x[i] = new float[m_height];
		tot_y[i] = new float[m_height];
		tot_z[i] = new float[m_height];
		sum_x[i] = new float[m_height];
		sum_y[i] = new float[m_height];
		sum_z[i] = new float[m_height];
		for(int j = 0; j < m_height; j++)
		{
			tot_x[i][j] = 0;
			tot_y[i][j] = 0;
			tot_z[i][j] = 0;
			sum_x[i][j] = 0;
			sum_y[i][j] = 0;
			sum_z[i][j] = 0;
		}
	}
	
	int size_box = 11;
	float smooth_radius = 0.025f*2.5f;

	int * wfound= new int[size_box*size_box];//Pre declaired for speed
	int * hfound= new int[size_box*size_box];

	int * xfound= new int[size_box*size_box];
	int * yfound= new int[size_box*size_box];
	int * zfound= new int[size_box*size_box];
	int nr_found = 0;

	real_2d_array x;
	real_1d_array y;
	real_1d_array c = "[1 , 1 , 1]";
	double epsf = 0;
	double epsx = 0.00001;
	ae_int_t maxits = 0;
	ae_int_t info;
	lsfitstate state;
	lsfitreport rep;
	double diffstep = 0.0001;


	for(int i = 0; i < m_width; i+=1)
	{
		//printf("-> %i\n",i);
		for(int j = 0; j < m_height; j+=1)
		{
			float x1 = m_points[i][j].x;
			float y1 = m_points[i][j].y;
			float z1 = m_points[i][j].z;

			int end_diff = 0;
			float sumx = 0; float totx = 0;
			float sumy = 0; float toty = 0;
			float sumz = 0; float totz = 0;
			float smooth_radius_dist = smooth_radius * z1;
			nr_found = 0;

			if(size_box%2 == 1){end_diff = 1;}
			if(z1 != 0)
			{
				int endbox_w = min(m_width,i+size_box/2+end_diff);
				int endbox_h = min(m_height,j+size_box/2+end_diff);
				int startbox_w = max(0,i-size_box/2);
				int startbox_h = max(0,j-size_box/2);
				for(int k = startbox_w; k < endbox_w; k+=1)
				{
					for(int l = startbox_h; l < endbox_h; l+=1)
					{
						int z2 = m_points[k][l].z;
						if(fabs(z2)>0.01f){
							float x2 = m_points[k][l].x;
							float y2 = m_points[k][l].y;
							float square_x_diff = (x1-x2)*(x1-x2);
							float square_y_diff = (y1-y2)*(y1-y2);
							float square_z_diff = (z1-z2)*(z1-z2);
							float dist_diff = sqrt(square_x_diff+square_y_diff+square_z_diff);

							if(dist_diff <= smooth_radius_dist){
								totx+=x2;toty+=y2;totz+=z2;
								wfound[nr_found]=k;
								hfound[nr_found]=l;
								xfound[nr_found]=x2;
								yfound[nr_found]=y2;
								zfound[nr_found]=z2;
								nr_found++;
							}

						}


					}
				}

				x.setlength(nr_found, 2);
				y.setlength(nr_found);

				for(int k = 0; k < nr_found; k++)
				{
					x(k,0)=xfound[k];
					x(k,1)=yfound[k];
					y(k)  =zfound[k];
				}
				bool success;
				try
				{
					success = true;
				}catch(...){info = 7;success = false;}


			}
		}
	}

	delete wfound;
	delete hfound;
	for(int i = 0; i < m_width;i++){
		delete tot_x[i];
		delete tot_y[i];
		delete tot_z[i];
		delete sum_x[i];
		delete sum_y[i];
		delete sum_z[i];
	}
	delete[] tot_x;
	delete[] tot_y;
	delete[] tot_z;
	delete[] sum_x;
	delete[] sum_y;
	delete[] sum_z;

	gettimeofday(&end, NULL);
	seconds  = end.tv_sec  - start.tv_sec;
	useconds = end.tv_usec - start.tv_usec;
	mtime = seconds*1000000  + useconds;
	if(useconds<0)
	{
		useconds += 1000000;
		seconds--;
	}
	printf("time filtering: %i.%i sek\n",seconds  ,useconds);*/
}

float offset = 1;//0.6;

//float offset = 1;

void
PointCloud::fillNormals()
{
//float offset = 1;
//offset-=0.01;
  /* First convert from our format to PCL format */
  pcl::PointCloud<pcl::PointXYZ> * cloud = new pcl::PointCloud<pcl::PointXYZ>;
  //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(ptr_to_be_able_to_clear_mem);
  cloud->width = m_width;
  cloud->height = m_height;
  cloud->points.resize(cloud->width * cloud->height);
  for (int i = 0; i < m_width; i++)
    {
      for (int j = 0; j < m_height; j++)
        {
          pcl::PointXYZ& pt = (*cloud)(i, j);
          if (m_points[i][j].z != 0)
            {
              (*cloud)(i, j).x = m_points[i][j].x;
              (*cloud)(i, j).y = m_points[i][j].y;
              (*cloud)(i, j).z = m_points[i][j].z;
            }
          else
            {
              pt.x = pt.y = pt.z = bad_point;
            }
        }
    }
  /* Now compute normals using PCL IntegralImageNormalEstimation */

  pcl::IntegralImageNormalEstimation normalEstimator_pointer;
  normalEstimator_pointer.setInputData(reinterpret_cast<float*> (&(cloud->points[0])), cloud->width, cloud->height, 3,
      sizeof(cloud->points[0]) / sizeof(float), (sizeof(cloud->points[0]) / sizeof(float)) * cloud->width, 10.0f);

  int rsize = 7;
  normalEstimator_pointer.setRectSize(rsize, rsize);
  for (int i = 0; i < cloud->width; i++)
    {
	//printf("i = %i\n",i);
      for (int j = 0; j < cloud->height; j++)
        {
	
          if (i > (1 + (rsize / 2)) && j > (1 + (rsize / 2)) && i <  cloud->width - (1 + (rsize / 2)) && j <  cloud->height - (1 + (rsize / 2)))
            {//printf("good:i = %i, j = %i\n",i,j);
              pcl::Normal normal = normalEstimator_pointer.compute(i, j);

              m_points[i][j].norm_x = normal.normal_x;
              m_points[i][j].norm_y = normal.normal_y;
              m_points[i][j].norm_z = normal.normal_z;
            }
          else
            {//printf("bad:i = %i, j = %i\n",i,j);
              m_points[i][j].norm_x = 0;
              m_points[i][j].norm_y = 0;
              m_points[i][j].norm_z = 0;
            }
        }
    }
//printf("filling normals \n");

	float ** print_matrix_r = new float *[640];
	float ** print_matrix_g = new float *[640];
	float ** print_matrix_b = new float *[640];
	for(int i = 0; i < 640; i++)
	{
		print_matrix_r[i]=new float[480];
		print_matrix_g[i]=new float[480];
		print_matrix_b[i]=new float[480];
	}

	for(int i = 0; i < this->m_width; i++)
	{
		for(int j = 0; j < this->m_height; j++)
		{
			//printf("%i %i -> ",i,j);
			Point pnt = this->m_points[i][j];
			if(pnt.is_valid){
				
				/*float offset_size = 0.01;
				if(pnt.x > offset-offset_size && pnt.x < offset+offset_size){
					print_matrix_r[i][j] = 256;
					print_matrix_g[i][j] = 256;
					print_matrix_b[i][j] = 256;
				}
				else{*/
					print_matrix_r[i][j] = (pnt.norm_x + 1)*128;
					print_matrix_g[i][j] = (pnt.norm_y + 1)*128;
					print_matrix_b[i][j] = (pnt.norm_z + 1)*128;
				//}

			}
			else{
				print_matrix_r[i][j] = -123;
				print_matrix_g[i][j] = -123;
				print_matrix_b[i][j] = -123;
			}
		}
	}

	show_rgb_matrix(print_matrix_r,print_matrix_g,print_matrix_b,640, 480);
	for(int i = 0; i < 640; i++)
	{
		delete[] print_matrix_r[i];
		delete[] print_matrix_g[i];
		delete[] print_matrix_b[i];
	}
	delete[] print_matrix_r;
	delete[] print_matrix_g;
	delete[] print_matrix_b;

delete cloud;
}

void
PointCloud::setValidPoints()
{
  for (int i = 0; i < m_width; i++)
    {
      for (int j = 0; j < m_height; j++)
        {
          m_points[i][j].is_valid = findobject::is_valid(&m_points[i][j]);
        }
    }
}

PointCloud::~PointCloud()
{
  // TODO Auto-generated destructor stub
  for (int i = 0; i < m_width; i++)
    {
	delete[] m_points[i];
    }
  delete[] m_points;
}


 Point& PointCloud::operator() (int width, int height)
 {
  if (width >= m_width || height >= m_height)
     printf("Matrix subscript out of bounds \n");
   return m_points[width][height];
 }

 Point PointCloud::operator() (int width, int height) const
 {
   if (width >= m_width || height >= m_height)
     printf("const Matrix subscript out of bounds \n");
   return m_points[width][height];
 }
#endif
